#pragma once

#include "hnurm_armor/Compensator.h"
#include "hnurm_armor/Processor.h"

#include "hnurm_interfaces/msg/armor_array.hpp"
#include "hnurm_interfaces/msg/target.hpp"
#include "hnurm_interfaces/msg/tracker_info.hpp"

#include <visualization_msgs/msg/marker_array.hpp>

#include "hnurm_armor/armor.hpp"
#include <mutex>
#include <thread>
#include <vector>

namespace hnurm
{
std::vector<Armor> fromROSMsg(const hnurm_interfaces::msg::ArmorArray &armors);

class ArmorNode : public rclcpp::Node
{
public:
    explicit ArmorNode(const rclcpp::NodeOptions &options);

    ~ArmorNode()
    {
        RCLCPP_INFO(get_logger(), "ArmorNode destroyed");
    }

    void init_markers();

    void send_target(TargetInfo &target, hnurm_interfaces::msg::VisionRecvData &uart_msg);

    void publish_markers(hnurm_interfaces::msg::Target &target_msg);

    void timer_callback();

    ArmorNode(const ArmorNode &)            = delete;
    ArmorNode &operator=(const ArmorNode &) = delete;
    ArmorNode(ArmorNode &&)                 = delete;
    ArmorNode &operator=(ArmorNode &&)      = delete;

protected:
    std::shared_ptr<ArmorNode> shared_from_this()
    {
        return std::static_pointer_cast<ArmorNode>(std::shared_ptr<rclcpp::Node>(this));
    }

private:
    void armor_array_callback(hnurm_interfaces::msg::ArmorArray::SharedPtr msg);

private:
    rclcpp::Publisher<hnurm_interfaces::msg::VisionSendData>::SharedPtr send_data_pub_;
    rclcpp::Subscription<hnurm_interfaces::msg::ArmorArray>::SharedPtr  armor_array_sub_;

    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
    rclcpp::Publisher<hnurm_interfaces::msg::Target>::SharedPtr        target_pub_;
    rclcpp::Publisher<hnurm_interfaces::msg::TrackerInfo>::SharedPtr   tracker_info_pub_;

    rclcpp::TimerBase::SharedPtr timer_;

    // markers
    visualization_msgs::msg::Marker position_marker_;
    visualization_msgs::msg::Marker linear_v_marker_;
    visualization_msgs::msg::Marker angular_v_marker_;
    visualization_msgs::msg::Marker armor_marker_;

    std::unique_ptr<Processor>   processor_;
    std::unique_ptr<Compensator> compensator_;
    std::vector<Armor>           armors;
    std::mutex                   mtx;

    hnurm_interfaces::msg::ArmorArray last_recv_array_;
    std::vector<float> pv;

    float calculateAverage(const std::vector<float> &vec);

    void Averagedistance(std::vector<float> &p, float value);
};
}